
	#include "QXCCommunications.h"
/******************************************************************************
*   							LOCAL MACROS
******************************************************************************/
	#define UART0		0
	#define UART1		1
	
/******************************************************************************
*   						PUBLIC  VARIABLES
******************************************************************************/
	// State Machine status for each port UART0 = A & UART1 = B		//
	 DisplaySM DisplayStatusA;
	 DisplaySM DisplayStatusB;
	
	// Variables to control Received Data from UART 0	//
	volatile char DataControlBufferA[30];
	volatile char *DataControlAPtr;
	char DataControlAEngage;
	char NewDataControlA;
	// Message change Flag for each Port	//
	 volatile char ChangeMessageTypeA;
	
	// Variables to control Received Data from UART 1	//
	volatile char DataControlBufferB[30];
	volatile char *DataControlBPtr;
	char DataControlBEngage;
	char NewDataControlB;
	// Message change Flag for each Port	//
	 volatile char ChangeMessageTypeB;
/******************************************************************************
*   						PRIVATE VARIABLES
******************************************************************************/
	#if DATA_STRUCTURE == TERMINAL
		static char DataTerminalBuffer[NUMBER_LINES*CARACTERS_LENGTH];
	#endif
	#if DATA_STRUCTURE == AHRS_GUI_TI || DATA_STRUCTURE == AHRS_GUI_QXC
		static char DataGUIBuffer[AHRS_DATA_LENGTH];
	#endif
	#if DATA_STRUCTURE == QXC_WIFI
		static char DataGUIBufferA[MESSAGE_LENGTH];
		static char DataGUIBufferB[MESSAGE_LENGTH];
	#endif
	
// Variables to control flow through UART 0	= A	//	
	volatile char MessageTypeA=0;
	volatile unsigned short CaracterCounterA=0;
	DisplaySM QXSTStatusA;
	DisplaySM QXINStatusA;
	DisplaySM QXDBStatusA;
	DisplaySM QXTIStatusA;
	DisplaySM QXMIStatusA;
	
// Variables to control flow through UART 1	= B	//
	volatile char MessageTypeB=0;
	volatile unsigned short CaracterCounterB=0;
	DisplaySM QXSTStatusB;
	DisplaySM QXINStatusB;
	DisplaySM QXDBStatusB;
	DisplaySM QXTIStatusB;
	DisplaySM QXMIStatusB;
	
	volatile short HeightCM_t,ProcessorTemperature_t;
	_iq BatteryStatus_t;

/******************************************************************************
*   					PUPLIC  FUNCTIONS DEFINITIONS
******************************************************************************/	
	// State Machine to write data to a PC terminal	//
	void VfnDisplayDataIntoTerminalA (void);
	
	// State Machine to write data to a WiFly / Xbee Module	//
	void VfnDisplayDataIntoTerminalB (void);
	
	// Function thta reset all state machines //
	void VfnDisplayDataInitialization (volatile unsigned char Port);
	
	// Function that Manage all message received from the Server.	//
	char VfnMessageManager (volatile unsigned char Port, char* BufferPtr, signed char* SetRoll, signed char* SetPitch, signed char* SetYaw, signed short* SetHeight, char* Hold);
	
/******************************************************************************
*   					PRIVATE FUNCTIONS DEFINITIONS
******************************************************************************/	
	// States for Machine A	//
	void TerminalDisplayAStateFill (void);
	void TerminalDisplayAStateDisplay(void);
	void TerminalDisplayAStateFinsh (void);
	
	// States for Machine B	//
	void TerminalDisplayBStateFill (void);
	void TerminalDisplayBStateDisplay(void);
	void TerminalDisplayBStateFinsh (void);
	
	void EulerAnlgesSection 		(char* BufferPortPtr,volatile char Portv);
	void SetEulerAnglesSection 		(char* BufferPortPtr,volatile char Port);
	void HeightSection 				(char* BufferPortPtr,volatile char Port);
	void OnBoardTemperatureSection 	(char* BufferPortPtr,volatile char Port);
	void BatterySection 			(char* BufferPortPtr,volatile char Port);
	void MotorsThrustSection 		(char* BufferPortPtr,volatile char Port);
	void ControlTypeSection 		(char* BufferPortPtr,volatile char Port);
	void EnviromentSensorSection 	(char* BufferPortPtr,volatile char Port);
	void PIDValuesSection 			(char* BufferPortPtr,volatile char Port);
	void PIDErrorValuesSection 		(char* BufferPortPtr,volatile char Port);
	
	// Introduce character Message Limiters at the begining and the end, <MESSAGE> //
	void VfnMessageLimiters (char *DataGUIBufferPtr);
	
	// DataGUIBuffer errase  function	//
	void VfnGUIBufferErase (char *DataGUIBufferPtr);
	
	// Function that split the Messag and, gives number of data splited and position
	void VfnParsedData(char *BufferPtr,unsigned char ParseCounter, char Spliter, unsigned char* DataCounter, char *DataPosition);

	// atoi Function	//
	short VfnMyAtoi(char* BufferPtr, unsigned char Counter);
	
	// Main State Machine A to Send a Data Message throug UART0 port = A //
	void (* const TerminalDisplayA[])(void);
	
	// Main State Machine A to Send a Data Message throug UART1 port = B //
	void (* const TerminalDisplayB[])(void);
	
	// State Machine to Fill up a Buffer wiht a specific Message information //
	// QXST Message Type
	void (* const FillQXSTTypeBuffer[]) (char* BufferPortPtr,volatile char Port);
	// QXIN Message Type
	void (* const FillQXINTypeBuffer[]) (char* BufferPortPtr,volatile char Port);
	// QXDB Message Type
	void (* const FillQXDBTypeBuffer[]) (char* BufferPortPtr,volatile char Port);
	// QXTI Message Type
	void (* const FillQXTITypeBuffer[]) (char* BufferPortPtr,volatile char Port);
	// QXMI Message Type
	void (* const FillQXMITypeBuffer[]) (char* BufferPortPtr,volatile char Port);

/******************************************************************************
*   						LOCAL FUNCTIONS
******************************************************************************/		
 /*****************************************************************************
 * Function:	VfnDisplayDataIntoTerminalA
 * Description:	Function/"EVENT" that run the State Machine to write "the System Status"/GUI sequence into a PC Terminal
 * Parameters:	
 * Caveats:		Main State Machine A
 * Return:		N/A
 *****************************************************************************/	
	void VfnDisplayDataIntoTerminalA (void)
	{	
		TerminalDisplayA[DisplayStatusA.ActualState]();
		if(ChangeMessageTypeA  == 1 && DisplayStatusA.ActualState == STATE_FINISH)
		{
			ChangeMessageTypeA = 0;
			VfnGUIBufferErase(&DataGUIBufferA[0]);
			VfnDisplayDataInitialization(UART0);	
		}	
	}

/*****************************************************************************
 * Function:	VfnDisplayDataIntoTerminalB
 * Description:	Function/"EVENT" that run the State Machine to write "the System Status"/GUI sequence into a Module
 * Parameters:	
 * Caveats:		Main State Machine B
 * Return:		N/A
 *****************************************************************************/	
	void VfnDisplayDataIntoTerminalB (void)
	{	
		TerminalDisplayB[DisplayStatusB.ActualState]();
		if(ChangeMessageTypeB  == 1 && DisplayStatusB.ActualState == STATE_FINISH)
		{
			ChangeMessageTypeB = 0;
			VfnGUIBufferErase(&DataGUIBufferB[0]);
			VfnDisplayDataInitialization(UART1);	
		}	
	}

/*****************************************************************************
 * Function:	VfnDisplayDataInitialization
 * Description:	Function to Reset all Variables and states machines.
 * Parameters:	Port variables to reset
 * Caveats:		
 * Return:		N/A
 *****************************************************************************/	
	void VfnDisplayDataInitialization (volatile unsigned char Port)
	{
		if(Port == UART0)
		{
			DisplayStatusA.ActualState=0;	
			QXSTStatusA.ActualState=0;
			QXINStatusA.ActualState=0;
			QXDBStatusA.ActualState=0;
			QXTIStatusA.ActualState=0;
			QXMIStatusA.ActualState=0;
			CaracterCounterA=0;
		}else{
			if(Port == UART1)
			{
				DisplayStatusB.ActualState=0;	
				QXSTStatusB.ActualState=0;
				QXINStatusB.ActualState=0;
				QXDBStatusB.ActualState=0;
				QXTIStatusB.ActualState=0;
				QXMIStatusB.ActualState=0;
				CaracterCounterB=0;
			}else{}
		}
	}

 /*****************************************************************************
 * Function:	VfnMessageManager 
 * Description:	Function that handle all messages coming from the server. Clissifies and take actions.
 * Parameters:	Are pointer to variable locations where to store the new commands.
 * 				*NOTE: new commands are avalibale only when function return a number 2.
 * Caveats:		
 * Return:		(1) when Server is requesting data, (0) when Server Stop requesting and, (2) when Server send New Commands
 *****************************************************************************/		
	char VfnMessageManager (volatile unsigned char Port, char *BufferPtr, signed char *SetRoll, signed char *SetPitch, signed char *SetYaw, signed short *SetHeight, char *Hold)
	{
		// Detect if first part of header is CS "Control Server"
		if(*BufferPtr == 'C' && *((BufferPtr)+1) == 'S')
		{
			// Detect is second part of header is IR "Information Request"
			if(*((BufferPtr)+2) == 'I' && *((BufferPtr)+3) == 'R')
			{
				if(Port == UART0)
				{
					BufferPtr = &DataControlBufferA[5];
				}else
				{
					if(Port == UART1)
					{
						BufferPtr = &DataControlBufferB[5];
					}else{}
				}
				// Detect first Message type == QXST
				// *Note - Only check for the first, third and fourth byte of the second part of the message
				if(*(BufferPtr) == 'Q' && *((BufferPtr)+2) == 'S' && *((BufferPtr)+3) == 'T' )
				{
					if(Port == UART0)
					{
						MessageTypeA = QXST;
						VfnGUIBufferErase(&DataControlBufferA[0]);
						return 1;
					}
					else{
						if(Port == UART1)
						{
							MessageTypeB = QXST;
							VfnGUIBufferErase(&DataControlBufferB[0]);
							return 1;
						}
						else{}
					}
				}
				// Detect first Message type == QXIN -> 
				if(*(BufferPtr) == 'Q' && *(BufferPtr+2) == 'I' && *(BufferPtr+3) == 'N' )
				{
					if(Port == UART0)
					{
						MessageTypeA = QXIN;
						VfnGUIBufferErase(&DataControlBufferA[0]);
						return 1;
					}
					else{
						if(Port == UART1)
						{
							MessageTypeB = QXIN;
							VfnGUIBufferErase(&DataControlBufferB[0]);
							return 1;
						}else{}
					}
				}
				// Detect first Message type == QXDB -> Debug information
				if(*(BufferPtr) == 'Q' && *(BufferPtr+2) == 'D' && *(BufferPtr+3) == 'B' )
				{
					if(Port == UART0)
					{
						MessageTypeA = QXDB;
						VfnGUIBufferErase(&DataControlBufferA[0]);
						return 1;
					}
					else{
						if(Port == UART1)
						{
							MessageTypeB = QXDB;
							VfnGUIBufferErase(&DataControlBufferB[0]);
							return 1;
						}else{}
					}
				}
				// Detect first Message type == QXTI -> Total information
				if(*(BufferPtr) == 'Q' && *(BufferPtr+2) == 'T' && *(BufferPtr+3) == 'I' )
				{
					if(Port == UART0)
					{
						MessageTypeA = QXTI;
						VfnGUIBufferErase(&DataControlBufferA[0]);
						return 1;
					}
					else{
						if(Port == UART1)
						{
							MessageTypeB = QXTI;
							VfnGUIBufferErase(&DataControlBufferB[0]);
							return 1;
						}else{}
					}
				}
				
				// Detect first Message type == QXMI -> Main information
				if(*(BufferPtr) == 'Q' && *(BufferPtr+2) == 'M' && *(BufferPtr+3) == 'I' )
				{
					if(Port == UART0)
					{
						MessageTypeA = QXMI;
						VfnGUIBufferErase(&DataControlBufferA[0]);
						return 1;
					}
					else{
						if(Port == UART1)
						{
							MessageTypeB = QXMI;
							VfnGUIBufferErase(&DataControlBufferB[0]);
							return 1;
						}else{}
					}
				}
				// Detect first Message type == QXSP -> Stop sending messages
				if(*(BufferPtr) == 'Q' && *(BufferPtr+2) == 'S' && *(BufferPtr+3) == 'P' )
				{
					if(Port == UART0)
					{
						MessageTypeA = QXSP;
						VfnGUIBufferErase(&DataControlBufferA[0]);
						return 0;
					}
					else{
						if(Port == UART1)
						{
							MessageTypeB = QXSP;
							VfnGUIBufferErase(&DataControlBufferB[0]);
							return 0;
						}else{}
					}
				}
			}else
			{
			
				// Detect is second part of header is CI "Commands Inputa"
				if(*(BufferPtr+2) == 'C' && *(BufferPtr+3) == 'I')
				{
					volatile char CounterD;
					volatile char PositionD[10];
					if(Port == UART0)
					{
						VfnParsedData(&DataControlBufferA[0],30,',',&CounterD,&PositionD[0]);
						*SetRoll = (signed char)VfnMyAtoi(&DataControlBufferA[ PositionD[1]],((PositionD[2]-PositionD[1])-1) );
						*SetPitch = (signed char)VfnMyAtoi(&DataControlBufferA[ PositionD[2]],((PositionD[3]-PositionD[2])-1) );
						*SetYaw = (signed char)VfnMyAtoi(&DataControlBufferA[ PositionD[3]],((PositionD[4]-PositionD[3])-1) );
						*SetHeight = (signed short)VfnMyAtoi(&DataControlBufferA[ PositionD[4]],((PositionD[5]-PositionD[4])-1) );
						*Hold = (char)VfnMyAtoi(&DataControlBufferA[ PositionD[5]],((PositionD[6]-PositionD[5])-1) );
						return 2;
					}else{
						if(Port == UART1)
						{
							VfnParsedData(&DataControlBufferB[0],30,',',&CounterD,&PositionD[0]);
							*SetRoll = (signed char)VfnMyAtoi(&DataControlBufferB[ PositionD[1]],((PositionD[2]-PositionD[1])-1) );
							*SetPitch = (signed char)VfnMyAtoi(&DataControlBufferB[ PositionD[2]],((PositionD[3]-PositionD[2])-1) );
							*SetYaw = (signed char)VfnMyAtoi(&DataControlBufferB[ PositionD[3]],((PositionD[4]-PositionD[3])-1) );
							*SetHeight = (signed short)VfnMyAtoi(&DataControlBufferB[ PositionD[4]],((PositionD[5]-PositionD[4])-1) );
							*Hold = (char)VfnMyAtoi(&DataControlBufferB[ PositionD[5]],((PositionD[6]-PositionD[5])-1) );
							return 2;
						}else{}
					}
				}
			}
		}
		return 0;	
	}

/*****************************************************************************
 * Function:	VfnMyAtoi
 * Description:	Convert a string number into a int/short number
 * Parameters:	String "Pointer" and, number of caracter to convert.
 * Caveats:		
 * Return:		number "Short type variable" / 0 if nothing to convert
 *****************************************************************************/
	short VfnMyAtoi(char* BufferPtr, unsigned char Counter)
	{
		unsigned char Count=0;
		volatile short number=0;
		volatile short Sign=1;
		while(Count != Counter)
		{
			if(*BufferPtr != '-')
			{
				if( *BufferPtr<'0' || *BufferPtr>'9')
				{
					return number;	
				}
				else
				{
					number = number * 10 + *BufferPtr - '0';
				}
				BufferPtr++;
				Count++;
			}
			else
			{
				BufferPtr++;
				Count++;
				Sign = -1;	
			}
		}
		return (number * Sign);
	}
	
/*****************************************************************************
 * Function:	TerminalDisplayAStateFill
 * Description:	STATE 0 - of the State machine that sends data through the UART0 Port into a PC terminal
 * 				STATE FILL - Load all data into DataTerminalBuffer.
 * Parameters:	
 * Caveats:		State Machine A
 * Return:		N/A
 *****************************************************************************/
	void TerminalDisplayAStateFill (void)
	{
		_iq valor;	
	#if DATA_STRUCTURE == TERMINAL
		unsigned char thrust;	
		sprintf((char*)&DataTerminalBuffer[ROLL_LINE*CARACTERS_LENGTH] , "ROLL        = ");
		sprintf((char*)&DataTerminalBuffer[PITCH_LINE*CARACTERS_LENGTH], "PITCH       = ");
		sprintf((char*)&DataTerminalBuffer[YAW_LINE*CARACTERS_LENGTH]  , "YAW         = ");
		sprintf((char*)&DataTerminalBuffer[TEMP_LINE*CARACTERS_LENGTH] , "TEMPERATURE = ");
		sprintf((char*)&DataTerminalBuffer[PTEMP_LINE*CARACTERS_LENGTH] , "ARM   TEMP. = ");
		sprintf((char*)&DataTerminalBuffer[BATTERY_LINE*CARACTERS_LENGTH] , "BATTERY VOLT= ");
		sprintf((char*)&DataTerminalBuffer[HEIGHT_LINE*CARACTERS_LENGTH] , "HEIGHT cm.  = ");
		
		thrust =  VfnGetQuadThrust();
		sprintf((char*)&DataTerminalBuffer[THRUST_LINE*CARACTERS_LENGTH],"QXC Thrust  = %d %%",thrust);
		
//		if(ControlCommandType == ANALOG_COMMANDS)
//		{
//			sprintf((char*)&DataTerminalBuffer[COMMAND_LINE*CARACTERS_LENGTH] , "COMMANDS TYP= Analog");
//		}
//		if(ControlCommandType == DIGITAL_COMMANDS)
//		{
//			sprintf((char*)&DataTerminalBuffer[COMMAND_LINE*CARACTERS_LENGTH] , "COMMANDS TYP= Digital");
//		}
		sprintf((char*)&DataTerminalBuffer[SET_ROLL_LINE*CARACTERS_LENGTH] , "SET ROLL    = ");
		sprintf((char*)&DataTerminalBuffer[SET_PITCH_LINE*CARACTERS_LENGTH] ,"SET PITCH   = ");
		sprintf((char*)&DataTerminalBuffer[SET_YAW_LINE*CARACTERS_LENGTH] ,  "SET YAW     = ");
		
		sprintf((char*)&DataTerminalBuffer[ROLL_ERROR_LINE*CARACTERS_LENGTH] , "ROLL ERROR  = ");
		sprintf((char*)&DataTerminalBuffer[PITCH_ERROR_LINE*CARACTERS_LENGTH] ,"PITCH ERROR = ");
		sprintf((char*)&DataTerminalBuffer[YAW_ERROR_LINE*CARACTERS_LENGTH] ,  "YAW ERROR   = ");
		sprintf((char*)&DataTerminalBuffer[ROLL_PID_LINE*CARACTERS_LENGTH] , "ROLL PID    = ");
		sprintf((char*)&DataTerminalBuffer[PITCH_PID_LINE*CARACTERS_LENGTH] ,"PITCH PID   = ");
		sprintf((char*)&DataTerminalBuffer[YAW_PID_LINE*CARACTERS_LENGTH] ,  "YAW PID     = ");
		
		DataTerminalBuffer[((PITCH_LINE+1)*CARACTERS_LENGTH)-1]=0x0D;
		DataTerminalBuffer[((ROLL_LINE+1)*CARACTERS_LENGTH)-1]=0x0D;
		DataTerminalBuffer[((YAW_LINE+1)*CARACTERS_LENGTH)-1]=0x0D;
		DataTerminalBuffer[((TEMP_LINE+1)*CARACTERS_LENGTH)-1]=0x0D;
		DataTerminalBuffer[((PTEMP_LINE+1)*CARACTERS_LENGTH)-1]=0x0D;
		DataTerminalBuffer[((BATTERY_LINE+1)*CARACTERS_LENGTH)-1]=0x0D;
		DataTerminalBuffer[((HEIGHT_LINE+1)*CARACTERS_LENGTH)-1]=0x0D;
		DataTerminalBuffer[((THRUST_LINE+1)*CARACTERS_LENGTH)-1]=0x0D;
		DataTerminalBuffer[((THRUST_LINE+1)*CARACTERS_LENGTH)-2]=0x0D;
		
		DataTerminalBuffer[((COMMAND_LINE+1)*CARACTERS_LENGTH)-1]=0x0D;
		DataTerminalBuffer[((SET_PITCH_LINE+1)*CARACTERS_LENGTH)-1]=0x0D;
		DataTerminalBuffer[((SET_ROLL_LINE+1)*CARACTERS_LENGTH)-1]=0x0D;
		DataTerminalBuffer[((SET_YAW_LINE+1)*CARACTERS_LENGTH)-1]=0x0D;
		DataTerminalBuffer[((SET_YAW_LINE+1)*CARACTERS_LENGTH)-1]=0x0D;
		DataTerminalBuffer[((SET_YAW_LINE+1)*CARACTERS_LENGTH)-2]=0x0D;
		
		DataTerminalBuffer[((PITCH_ERROR_LINE+1)*CARACTERS_LENGTH)-1]=0x0D;
		DataTerminalBuffer[((PITCH_PID_LINE+1)*CARACTERS_LENGTH)-1]=0x0D;
		DataTerminalBuffer[((ROLL_ERROR_LINE+1)*CARACTERS_LENGTH)-1]=0x0D;
		DataTerminalBuffer[((ROLL_PID_LINE+1)*CARACTERS_LENGTH)-1]=0x0D;
		DataTerminalBuffer[((YAW_ERROR_LINE+1)*CARACTERS_LENGTH)-1]=0x0D;
		DataTerminalBuffer[((YAW_PID_LINE+1)*CARACTERS_LENGTH)-1]=0x0D;
		DataTerminalBuffer[((YAW_PID_LINE+1)*CARACTERS_LENGTH)-2]=0x0D;
		
		valor = _IQmpy(Roll,_IQ(RAD_TO_DEGREES));
		_IQtoa(&DataTerminalBuffer[(ROLL_LINE*CARACTERS_LENGTH)+DATA_START],"%5.6f",valor);
		valor = _IQmpy(Pitch,_IQ(RAD_TO_DEGREES));
		_IQtoa(&DataTerminalBuffer[(PITCH_LINE*CARACTERS_LENGTH)+DATA_START],"%5.6f",valor);
		valor = _IQmpy(Yaw,_IQ(RAD_TO_DEGREES));
		_IQtoa(&DataTerminalBuffer[(YAW_LINE*CARACTERS_LENGTH)+DATA_START],"%5.6f",valor);
		VfnDS16XXTemperatureToString(&DataTerminalBuffer[(TEMP_LINE*CARACTERS_LENGTH)+DATA_START],DS1624Temp);
		sprintf((char*)&DataTerminalBuffer[(PTEMP_LINE*CARACTERS_LENGTH)+DATA_START] ,  "%d",ProcessorTemperature_t);
		_IQtoa(&DataTerminalBuffer[(BATTERY_LINE*CARACTERS_LENGTH)+DATA_START],"%4.3f",BatteryStatus_t);
		sprintf((char*)&DataTerminalBuffer[(HEIGHT_LINE*CARACTERS_LENGTH)+DATA_START] ,  "%d",HeightCM_t);
		
		valor = _IQmpy(SetRollAngle,_IQ(RAD_TO_DEGREES));
		_IQtoa(&DataTerminalBuffer[(SET_ROLL_LINE*CARACTERS_LENGTH)+DATA_START],"%5.6f",valor);
		valor = _IQmpy(SetPitchAngle,_IQ(RAD_TO_DEGREES));
		_IQtoa(&DataTerminalBuffer[(SET_PITCH_LINE*CARACTERS_LENGTH)+DATA_START],"%5.6f",valor);
		valor = _IQmpy(SetYawAngle,_IQ(RAD_TO_DEGREES));
		_IQtoa(&DataTerminalBuffer[(SET_YAW_LINE*CARACTERS_LENGTH)+DATA_START],"%5.6f",valor);
		
		_IQtoa(&DataTerminalBuffer[(PITCH_ERROR_LINE*CARACTERS_LENGTH)+DATA_START],"%5.6f",ErrorPitch);
		_IQtoa(&DataTerminalBuffer[(ROLL_ERROR_LINE*CARACTERS_LENGTH)+DATA_START],"%5.6f",ErrorRoll);
		_IQtoa(&DataTerminalBuffer[(YAW_ERROR_LINE*CARACTERS_LENGTH)+DATA_START],"%5.6f",ErrorYaw);
		
		_IQtoa(&DataTerminalBuffer[(PITCH_PID_LINE*CARACTERS_LENGTH)+DATA_START],"%5.6f",PidP);
		_IQtoa(&DataTerminalBuffer[(ROLL_PID_LINE*CARACTERS_LENGTH)+DATA_START],"%5.6f",PidR);
		_IQtoa(&DataTerminalBuffer[(YAW_PID_LINE*CARACTERS_LENGTH)+DATA_START],"%5.6f",PidY);
		DisplayStatusA.ActualState = STATE_DISPLAY;
	#endif
	
	#if DATA_STRUCTURE == AHRS_GUI_TI
		valor = _IQmpy(Roll,_IQ(RAD_TO_DEGREES));
		_IQtoa(&DataGUIBuffer[ROLL_ANGLE],"%5.4f",valor);
		DataGUIBuffer[PITCH_ANGLE-1] = ',';
	
		valor = _IQmpy(Pitch,_IQ(RAD_TO_DEGREES));
		_IQtoa(&DataGUIBuffer[PITCH_ANGLE],"%5.4f",valor);
		DataGUIBuffer[YAW_ANGLE-1] = ',';	
		
		valor = _IQmpy(Yaw,_IQ(RAD_TO_DEGREES));
		_IQtoa(&DataGUIBuffer[YAW_ANGLE],"%5.4f",valor);
	
		DataGUIBuffer[AHRS_DATA_LENGTH-1] = '!';
		DisplayStatusA.ActualState = STATE_DISPLAY;
	#endif	
		
	#if DATA_STRUCTURE == QXC_WIFI
		if( MessageTypeA == QXST )
		{
			FillQXSTTypeBuffer[QXSTStatusA.ActualState](&DataGUIBufferA[0],UART0);
			if(QXSTStatusA.ActualState == SECTION7)
			{
				DataGUIBufferA[ ((CONTROL_TYPE*DATA_LENGTH)+DATA_LENGTH)+4] = 0;
				VfnMessageLimiters(&DataGUIBufferA[0]);
				VfnDisplayDataInitialization(UART0);
				DisplayStatusA.ActualState = STATE_DISPLAY;
			}
		}
		if( MessageTypeA == QXIN )
		{
			FillQXINTypeBuffer[QXINStatusA.ActualState](&DataGUIBufferA[0],UART0);
			if(QXINStatusA.ActualState == SECTION2)
			{
				DataGUIBufferA[ ((TEMPERATURE*DATA_LENGTH)+DATA_LENGTH)+4] = 0;
				VfnMessageLimiters(&DataGUIBufferA[0]);
				VfnDisplayDataInitialization(UART0);
				DisplayStatusA.ActualState = STATE_DISPLAY;
			}
		}
		if( MessageTypeA == QXDB )
		{
			FillQXDBTypeBuffer[QXDBStatusA.ActualState](&DataGUIBufferA[0],UART0);
			if(QXDBStatusA.ActualState == SECTION3)
			{
				DataGUIBufferA[((YAW_ERROR*DATA_LENGTH)+DATA_LENGTH)+4] = 0;
				VfnMessageLimiters(&DataGUIBufferA[0]);
				VfnDisplayDataInitialization(UART0);
				DisplayStatusA.ActualState = STATE_DISPLAY;
			}
		}
		if( MessageTypeA == QXTI )
		{
			FillQXTITypeBuffer[QXTIStatusA.ActualState](&DataGUIBufferA[0],UART0);
			if(QXTIStatusA.ActualState == SECTION_MAX)
			{
				DataGUIBufferA[((YAW_ERROR*DATA_LENGTH)+DATA_LENGTH)+4] = 0;
				VfnMessageLimiters(&DataGUIBufferA[0]);
				VfnDisplayDataInitialization(UART0);
				DisplayStatusA.ActualState = STATE_DISPLAY;
			}
		}
		if( MessageTypeA == QXMI )
		{
			FillQXMITypeBuffer[QXMIStatusA.ActualState](&DataGUIBufferA[0],UART0);
			if(QXMIStatusA.ActualState == SECTION8)
			{
				DataGUIBufferA[ ((TEMPERATURE*DATA_LENGTH)+DATA_LENGTH)+4] = 0;
				VfnMessageLimiters(&DataGUIBufferA[0]);
				VfnDisplayDataInitialization(UART0);
				DisplayStatusA.ActualState = STATE_DISPLAY;
			}
		}
		if( !(MessageTypeA == QXST || MessageTypeA == QXMI || MessageTypeA == QXTI || MessageTypeA == QXIN || MessageTypeA == QXDB))
		{
			DisplayStatusA.ActualState = STATE_FINISH;
		}
	#endif
	}

/*****************************************************************************
 * Function:	TerminalDisplayStateDisplay
 * Description:	STATE 1 - of the State machine that sends data through the UART0 Port into a PC terminal
 * 				STATE DISPLAY - Sends all data contain in the DataTerminalBuffer[] through the UART0 port.
 * 				MAX_DISPLAY determine the number of caracters send by the UART0 port.
 * 				once caracter counter has reach MAX_DISPLAY. avance to next state "Finish"
 * Parameters:	
 * Caveats:		State Machine A
 * Return:		N/A
 *****************************************************************************/
	void TerminalDisplayAStateDisplay (void)
	{
		char FIFOFlag;
	#if DATA_STRUCTURE == TERMINAL
		//if Actual data in the buffer is equal to 0, then skip data.
		if(DataTerminalBuffer[CaracterCounter]!=0)
		{
			// if actual data is not equal to 0, send it through the UART 0 port
			FIFOFlag = UARTCharPutNonBlocking(UART0_BASE,DataTerminalBuffer[CaracterCounter]);
			// if FIFOFlag is equal to 1, it means a succesful sending by the UARt port
			if(FIFOFlag == 1)
			{
				CaracterCounter++;	
				if(CaracterCounter == MAX_DISPLAY)
				{
					// if CaracterCounter is equalt to the Total MAX DISPLAY,
					// Advance to the next state
					CaracterCounter=0;
					DisplayStatusA.ActualState = STATE_FINISH;
				}
			}
		}
		else
		{
			CaracterCounter++;
		}
	#endif
	#if DATA_STRUCTURE == AHRS_GUI_TI 
	//if Actual data in the buffer is equal to 0, then skip data.
		if(DataGUIBuffer[CaracterCounter]!=0)
		{
			// if actual data is not equal to 0, send it through the UART 0 port
			FIFOFlag = UARTCharPutNonBlocking(UART0_BASE,DataGUIBuffer[CaracterCounter]);
			// if FIFOFlag is equal to 1, it means a succesful sending by the UARt port
			if(FIFOFlag == 1)
			{
				CaracterCounter++;	
				if(CaracterCounter == (AHRS_DATA_LENGTH))
				{
					// if CaracterCounter is equalt to the Total MAX DISPLAY,
					// Advance to the next state
					CaracterCounter=0;
					DisplayStatusA.ActualState = STATE_FINISH;
				}
			}
		}
		else
		{
			CaracterCounter++;
		}
	#endif
	
	#if DATA_STRUCTURE == QXC_WIFI
	//////////////////////////////////////////////////////////
	//		SEND DATA THROUGH UART PORT 0 = A - CODE		//
	//////////////////////////////////////////////////////////
	//if Actual data in the buffer is equal to 0, then skip data.
		if(DataGUIBufferA[CaracterCounterA]!=0)
		{
			// if actual data is not equal to 0, send it through the UART 0 port
			FIFOFlag = UARTCharPutNonBlocking(UART0_BASE,DataGUIBufferA[CaracterCounterA]);
			// if FIFOFlag is equal to 1, it means a succesful sending by the UARt port
			if(FIFOFlag == 1)
			{
				// if CaracterCounter is equalt to the Total MAX DISPLAY,
					// Advance to the next state
				CaracterCounterA++;	
				if(CaracterCounterA == (MESSAGE_LENGTH))
				{
					CaracterCounterA=0;
					DisplayStatusA.ActualState = STATE_FINISH;
				}
			}
		}
		else
		{
			CaracterCounterA++;
		}
	#endif
	}
	
/*****************************************************************************
 * Function:	TerminalDisplayAStateFinish
 * Description:	STATE 2 - of the State machine that sends data through the UART0 Port into a PC terminal
 * 				STATE FINISH - Reset CaracterCounter.
 * Parameters:	
 * Caveats:		State Machine A - UART0
 * Return:		N/A
 *****************************************************************************/
	void TerminalDisplayAStateFinsh (void)
	{
		CaracterCounterA=0;
		VfnGUIBufferErase (&DataGUIBufferA[0]);
	}
	
/*****************************************************************************
 * Function:	void (* const TerminalDisplay[])(void)
 * Description:	Functions Pointer - that conform the TerminalDisplay State Machine.
 * Parameters:	
 * Caveats:		State Machine A - UART0
 * Return:		N/A
 *****************************************************************************/
	void (* const TerminalDisplayA[])(void) =
	{
		TerminalDisplayAStateFill,
		TerminalDisplayAStateDisplay,
		TerminalDisplayAStateFinsh
	};
	
/*****************************************************************************
 * Function:	TerminalDisplayBStateFill
 * Description:	STATE 0 - of the State machine that sends data through the UART1 Port into a PC terminal
 * 				STATE FILL - Load all data into DataTerminalBuffer.
 * Parameters:	
 * Caveats:		State Machine B - UART1
 * Return:		N/A
 *****************************************************************************/
	void TerminalDisplayBStateFill (void)
	{
		_iq valor;	
	#if DATA_STRUCTURE == TERMINAL
		unsigned char thrust;	
		sprintf((char*)&DataTerminalBuffer[ROLL_LINE*CARACTERS_LENGTH] , "ROLL        = ");
		sprintf((char*)&DataTerminalBuffer[PITCH_LINE*CARACTERS_LENGTH], "PITCH       = ");
		sprintf((char*)&DataTerminalBuffer[YAW_LINE*CARACTERS_LENGTH]  , "YAW         = ");
		sprintf((char*)&DataTerminalBuffer[TEMP_LINE*CARACTERS_LENGTH] , "TEMPERATURE = ");
		sprintf((char*)&DataTerminalBuffer[PTEMP_LINE*CARACTERS_LENGTH] , "ARM   TEMP. = ");
		sprintf((char*)&DataTerminalBuffer[BATTERY_LINE*CARACTERS_LENGTH] , "BATTERY VOLT= ");
		sprintf((char*)&DataTerminalBuffer[HEIGHT_LINE*CARACTERS_LENGTH] , "HEIGHT cm.  = ");
		
		thrust =  VfnGetQuadThrust();
		sprintf((char*)&DataTerminalBuffer[THRUST_LINE*CARACTERS_LENGTH],"QXC Thrust  = %d %%",thrust);
		
//		if(ControlCommandType == ANALOG_COMMANDS)
//		{
//			sprintf((char*)&DataTerminalBuffer[COMMAND_LINE*CARACTERS_LENGTH] , "COMMANDS TYP= Analog");
//		}
//		if(ControlCommandType == DIGITAL_COMMANDS)
//		{
//			sprintf((char*)&DataTerminalBuffer[COMMAND_LINE*CARACTERS_LENGTH] , "COMMANDS TYP= Digital");
//		}
		sprintf((char*)&DataTerminalBuffer[SET_ROLL_LINE*CARACTERS_LENGTH] , "SET ROLL    = ");
		sprintf((char*)&DataTerminalBuffer[SET_PITCH_LINE*CARACTERS_LENGTH] ,"SET PITCH   = ");
		sprintf((char*)&DataTerminalBuffer[SET_YAW_LINE*CARACTERS_LENGTH] ,  "SET YAW     = ");
		
		sprintf((char*)&DataTerminalBuffer[ROLL_ERROR_LINE*CARACTERS_LENGTH] , "ROLL ERROR  = ");
		sprintf((char*)&DataTerminalBuffer[PITCH_ERROR_LINE*CARACTERS_LENGTH] ,"PITCH ERROR = ");
		sprintf((char*)&DataTerminalBuffer[YAW_ERROR_LINE*CARACTERS_LENGTH] ,  "YAW ERROR   = ");
		sprintf((char*)&DataTerminalBuffer[ROLL_PID_LINE*CARACTERS_LENGTH] , "ROLL PID    = ");
		sprintf((char*)&DataTerminalBuffer[PITCH_PID_LINE*CARACTERS_LENGTH] ,"PITCH PID   = ");
		sprintf((char*)&DataTerminalBuffer[YAW_PID_LINE*CARACTERS_LENGTH] ,  "YAW PID     = ");
		
		DataTerminalBuffer[((PITCH_LINE+1)*CARACTERS_LENGTH)-1]=0x0D;
		DataTerminalBuffer[((ROLL_LINE+1)*CARACTERS_LENGTH)-1]=0x0D;
		DataTerminalBuffer[((YAW_LINE+1)*CARACTERS_LENGTH)-1]=0x0D;
		DataTerminalBuffer[((TEMP_LINE+1)*CARACTERS_LENGTH)-1]=0x0D;
		DataTerminalBuffer[((PTEMP_LINE+1)*CARACTERS_LENGTH)-1]=0x0D;
		DataTerminalBuffer[((BATTERY_LINE+1)*CARACTERS_LENGTH)-1]=0x0D;
		DataTerminalBuffer[((HEIGHT_LINE+1)*CARACTERS_LENGTH)-1]=0x0D;
		DataTerminalBuffer[((THRUST_LINE+1)*CARACTERS_LENGTH)-1]=0x0D;
		DataTerminalBuffer[((THRUST_LINE+1)*CARACTERS_LENGTH)-2]=0x0D;
		
		DataTerminalBuffer[((COMMAND_LINE+1)*CARACTERS_LENGTH)-1]=0x0D;
		DataTerminalBuffer[((SET_PITCH_LINE+1)*CARACTERS_LENGTH)-1]=0x0D;
		DataTerminalBuffer[((SET_ROLL_LINE+1)*CARACTERS_LENGTH)-1]=0x0D;
		DataTerminalBuffer[((SET_YAW_LINE+1)*CARACTERS_LENGTH)-1]=0x0D;
		DataTerminalBuffer[((SET_YAW_LINE+1)*CARACTERS_LENGTH)-1]=0x0D;
		DataTerminalBuffer[((SET_YAW_LINE+1)*CARACTERS_LENGTH)-2]=0x0D;
		
		DataTerminalBuffer[((PITCH_ERROR_LINE+1)*CARACTERS_LENGTH)-1]=0x0D;
		DataTerminalBuffer[((PITCH_PID_LINE+1)*CARACTERS_LENGTH)-1]=0x0D;
		DataTerminalBuffer[((ROLL_ERROR_LINE+1)*CARACTERS_LENGTH)-1]=0x0D;
		DataTerminalBuffer[((ROLL_PID_LINE+1)*CARACTERS_LENGTH)-1]=0x0D;
		DataTerminalBuffer[((YAW_ERROR_LINE+1)*CARACTERS_LENGTH)-1]=0x0D;
		DataTerminalBuffer[((YAW_PID_LINE+1)*CARACTERS_LENGTH)-1]=0x0D;
		DataTerminalBuffer[((YAW_PID_LINE+1)*CARACTERS_LENGTH)-2]=0x0D;
		
		valor = _IQmpy(Roll,_IQ(RAD_TO_DEGREES));
		_IQtoa(&DataTerminalBuffer[(ROLL_LINE*CARACTERS_LENGTH)+DATA_START],"%5.6f",valor);
		valor = _IQmpy(Pitch,_IQ(RAD_TO_DEGREES));
		_IQtoa(&DataTerminalBuffer[(PITCH_LINE*CARACTERS_LENGTH)+DATA_START],"%5.6f",valor);
		valor = _IQmpy(Yaw,_IQ(RAD_TO_DEGREES));
		_IQtoa(&DataTerminalBuffer[(YAW_LINE*CARACTERS_LENGTH)+DATA_START],"%5.6f",valor);
		VfnDS16XXTemperatureToString(&DataTerminalBuffer[(TEMP_LINE*CARACTERS_LENGTH)+DATA_START],DS1624Temp);
		sprintf((char*)&DataTerminalBuffer[(PTEMP_LINE*CARACTERS_LENGTH)+DATA_START] ,  "%d",ProcessorTemperature_t);
		_IQtoa(&DataTerminalBuffer[(BATTERY_LINE*CARACTERS_LENGTH)+DATA_START],"%4.3f",BatteryStatus_t);
		sprintf((char*)&DataTerminalBuffer[(HEIGHT_LINE*CARACTERS_LENGTH)+DATA_START] ,  "%d",HeightCM_t);
		
		valor = _IQmpy(SetRollAngle,_IQ(RAD_TO_DEGREES));
		_IQtoa(&DataTerminalBuffer[(SET_ROLL_LINE*CARACTERS_LENGTH)+DATA_START],"%5.6f",valor);
		valor = _IQmpy(SetPitchAngle,_IQ(RAD_TO_DEGREES));
		_IQtoa(&DataTerminalBuffer[(SET_PITCH_LINE*CARACTERS_LENGTH)+DATA_START],"%5.6f",valor);
		valor = _IQmpy(SetYawAngle,_IQ(RAD_TO_DEGREES));
		_IQtoa(&DataTerminalBuffer[(SET_YAW_LINE*CARACTERS_LENGTH)+DATA_START],"%5.6f",valor);
		
		_IQtoa(&DataTerminalBuffer[(PITCH_ERROR_LINE*CARACTERS_LENGTH)+DATA_START],"%5.6f",ErrorPitch);
		_IQtoa(&DataTerminalBuffer[(ROLL_ERROR_LINE*CARACTERS_LENGTH)+DATA_START],"%5.6f",ErrorRoll);
		_IQtoa(&DataTerminalBuffer[(YAW_ERROR_LINE*CARACTERS_LENGTH)+DATA_START],"%5.6f",ErrorYaw);
		
		_IQtoa(&DataTerminalBuffer[(PITCH_PID_LINE*CARACTERS_LENGTH)+DATA_START],"%5.6f",PidP);
		_IQtoa(&DataTerminalBuffer[(ROLL_PID_LINE*CARACTERS_LENGTH)+DATA_START],"%5.6f",PidR);
		_IQtoa(&DataTerminalBuffer[(YAW_PID_LINE*CARACTERS_LENGTH)+DATA_START],"%5.6f",PidY);
		DisplayStatusA.ActualState = STATE_DISPLAY;
	#endif
	
	#if DATA_STRUCTURE == AHRS_GUI_TI
		valor = _IQmpy(Roll,_IQ(RAD_TO_DEGREES));
		_IQtoa(&DataGUIBuffer[ROLL_ANGLE],"%5.4f",valor);
		DataGUIBuffer[PITCH_ANGLE-1] = ',';
	
		valor = _IQmpy(Pitch,_IQ(RAD_TO_DEGREES));
		_IQtoa(&DataGUIBuffer[PITCH_ANGLE],"%5.4f",valor);
		DataGUIBuffer[YAW_ANGLE-1] = ',';	
		
		valor = _IQmpy(Yaw,_IQ(RAD_TO_DEGREES));
		_IQtoa(&DataGUIBuffer[YAW_ANGLE],"%5.4f",valor);
	
		DataGUIBuffer[AHRS_DATA_LENGTH-1] = '!';
		DisplayStatusA.ActualState = STATE_DISPLAY;
	#endif	
			
	#if DATA_STRUCTURE == QXC_WIFI
		if( MessageTypeB == QXST )
		{
			FillQXSTTypeBuffer[QXSTStatusB.ActualState](&DataGUIBufferB[0],UART1);
			if(QXSTStatusB.ActualState == SECTION7)
			{
				DataGUIBufferB[ ((CONTROL_TYPE*DATA_LENGTH)+DATA_LENGTH)+4] = 0;
				VfnMessageLimiters(&DataGUIBufferB[0]);
				VfnDisplayDataInitialization(UART1);
				DisplayStatusB.ActualState = STATE_DISPLAY;
			}
		}
		if( MessageTypeB == QXIN )
		{
			FillQXINTypeBuffer[QXINStatusB.ActualState](&DataGUIBufferB[0],UART1);
			if(QXINStatusB.ActualState == SECTION2)
			{
				DataGUIBufferB[ ((TEMPERATURE*DATA_LENGTH)+DATA_LENGTH)+4] = 0;
				VfnMessageLimiters(&DataGUIBufferB[0]);
				VfnDisplayDataInitialization(UART1);
				DisplayStatusB.ActualState = STATE_DISPLAY;
			}
		}
		if( MessageTypeB == QXDB )
		{
			FillQXDBTypeBuffer[QXDBStatusB.ActualState](&DataGUIBufferB[0],UART1);
			if(QXDBStatusB.ActualState == SECTION3)
			{
				DataGUIBufferB[((YAW_ERROR*DATA_LENGTH)+DATA_LENGTH)+4] = 0;
				VfnMessageLimiters(&DataGUIBufferB[0]);
				VfnDisplayDataInitialization(UART1);
				DisplayStatusB.ActualState = STATE_DISPLAY;
			}
		}
		if( MessageTypeB == QXTI )
		{
			FillQXTITypeBuffer[QXTIStatusB.ActualState](&DataGUIBufferB[0],UART1);
			if(QXTIStatusB.ActualState == SECTION_MAX)
			{
				DataGUIBufferB[((YAW_ERROR*DATA_LENGTH)+DATA_LENGTH)+4] = 0;
				VfnMessageLimiters(&DataGUIBufferB[0]);
				VfnDisplayDataInitialization(UART1);
				DisplayStatusB.ActualState = STATE_DISPLAY;
			}
		}
		if( MessageTypeB == QXMI )
		{
			FillQXMITypeBuffer[QXMIStatusB.ActualState](&DataGUIBufferB[0],UART1);
			if(QXMIStatusB.ActualState == SECTION8)
			{
				DataGUIBufferB[ ((TEMPERATURE*DATA_LENGTH)+DATA_LENGTH)+4] = 0;
				VfnMessageLimiters(&DataGUIBufferB[0]);
				VfnDisplayDataInitialization(UART1);
				DisplayStatusB.ActualState = STATE_DISPLAY;
			}
		}
		if( !(MessageTypeB == QXST || MessageTypeB == QXMI || MessageTypeB == QXTI || MessageTypeB == QXIN || MessageTypeB == QXDB))
		{
			DisplayStatusB.ActualState = STATE_FINISH;
		}
	#endif
	}

/*****************************************************************************
 * Function:	TerminalDisplayStateDisplay
 * Description:	STATE 1 - of the State machine that sends data through the UART1 Port into a PC terminal
 * 				STATE DISPLAY - Sends all data contain in the DataTerminalBuffer[] through the UART1 port.
 * 				MAX_DISPLAY determine the number of caracters send by the UART1 port.
 * 				once caracter counter has reach MAX_DISPLAY. avance to next state "Finish"
 * Parameters:	
 * Caveats:		State Machine B - UART1
 * Return:		N/A
 *****************************************************************************/
	void TerminalDisplayBStateDisplay (void)
	{
		char FIFOFlag;
	#if DATA_STRUCTURE == TERMINAL
		//if Actual data in the buffer is equal to 0, then skip data.
		if(DataTerminalBuffer[CaracterCounter]!=0)
		{
			// if actual data is not equal to 0, send it through the UART 0 port
			FIFOFlag = UARTCharPutNonBlocking(UART0_BASE,DataTerminalBuffer[CaracterCounter]);
			// if FIFOFlag is equal to 1, it means a succesful sending by the UARt port
			if(FIFOFlag == 1)
			{
				CaracterCounter++;	
				if(CaracterCounter == MAX_DISPLAY)
				{
					// if CaracterCounter is equalt to the Total MAX DISPLAY,
					// Advance to the next state
					CaracterCounter=0;
					DisplayStatusA.ActualState = STATE_FINISH;
				}
			}
		}
		else
		{
			CaracterCounter++;
		}
	#endif
	#if DATA_STRUCTURE == AHRS_GUI_TI || DATA_STRUCTURE == AHRS_GUI_QXC
	//if Actual data in the buffer is equal to 0, then skip data.
		if(DataGUIBufferA[CaracterCounter]!=0)
		{
			// if actual data is not equal to 0, send it through the UART 0 port
			FIFOFlag = UARTCharPutNonBlocking(UART0_BASE,DataGUIBufferA[CaracterCounter]);
			// if FIFOFlag is equal to 1, it means a succesful sending by the UARt port
			if(FIFOFlag == 1)
			{
				CaracterCounter++;	
				if(CaracterCounter == (AHRS_DATA_LENGTH))
				{
					// if CaracterCounter is equalt to the Total MAX DISPLAY,
					// Advance to the next state
					CaracterCounter=0;
					DisplayStatusA.ActualState = STATE_FINISH;
				}
			}
		}
		else
		{
			CaracterCounter++;
		}
	#endif
	
	#if DATA_STRUCTURE == QXC_WIFI
	//////////////////////////////////////////////////////////
	//		SEND DATA THROUGH UART PORT 1 = B - CODE		//
	//////////////////////////////////////////////////////////
	//if Actual data in the buffer is equal to 0, then skip data.
		if(DataGUIBufferB[CaracterCounterB]!=0)
		{
			// if actual data is not equal to 0, send it through the UART 1 port
			FIFOFlag = UARTCharPutNonBlocking(UART1_BASE,DataGUIBufferB[CaracterCounterB]);
			// if FIFOFlag is equal to 1, it means a succesful sending by the UARt port
			if(FIFOFlag == 1)
			{
				// if CaracterCounter is equalt to the Total MAX DISPLAY,
					// Advance to the next state
				CaracterCounterB++;	
				if(CaracterCounterB == (MESSAGE_LENGTH))
				{
					CaracterCounterB=0;
					DisplayStatusB.ActualState = STATE_FINISH;
				}
			}
		}
		else
		{
			CaracterCounterB++;
		}
	#endif
	}
	
/*****************************************************************************
 * Function:	TerminalDisplayBStateFinish
 * Description:	STATE 2 - of the State machine that sends data through the UART1 Port into a PC terminal
 * 				STATE FINISH - Reset CaracterCounter.
 * Parameters:	
 * Caveats:		State Machine B - UART1
 * Return:		N/A
 *****************************************************************************/
	void TerminalDisplayBStateFinsh (void)
	{
		CaracterCounterB=0;
		VfnGUIBufferErase (&DataGUIBufferB[0]);
	}
	
/*****************************************************************************
 * Function:	void (* const TerminalDisplayB[])(void)
 * Description:	Functions Pointer - that conform the TerminalDisplay State Machine.
 * Parameters:	
 * Caveats:		State Machine B - UART1
 * Return:		N/A
 *****************************************************************************/
	void (* const TerminalDisplayB[])(void) =
	{
		TerminalDisplayBStateFill,
		TerminalDisplayBStateDisplay,
		TerminalDisplayBStateFinsh
	};

/////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////
#if DATA_STRUCTURE == QXC_WIFI
	void EulerAnlgesSection (char* BufferPortPtr,volatile char Port)
	{
		_iq valor;
		valor = _IQmpy(Roll,_IQ(RAD_TO_DEGREES));
		_IQtoa( ((BufferPortPtr) + ((ROLL_ANGLE * DATA_LENGTH)+1)+5 ),"%4.2f",valor);
		*( (BufferPortPtr) + ((ROLL_ANGLE*DATA_LENGTH)+DATA_LENGTH)+4 ) = ',';

		valor = _IQmpy(Pitch,_IQ(RAD_TO_DEGREES));
		_IQtoa( ((BufferPortPtr) + (PITCH_ANGLE * DATA_LENGTH)+5 ),"%4.2f",valor);
		*( (BufferPortPtr) + ((PITCH_ANGLE*DATA_LENGTH)+DATA_LENGTH)+4 ) = ',';	

		valor = _IQmpy(Yaw,_IQ(RAD_TO_DEGREES));
		_IQtoa( ((BufferPortPtr) + (YAW_ANGLE * DATA_LENGTH)+5 ),"%4.2f",valor);
		*( (BufferPortPtr) + ((YAW_ANGLE*DATA_LENGTH)+DATA_LENGTH)+4 )= ',';
		
		//Next State
		if(Port == UART0)
		{
			QXSTStatusA.ActualState +=1;
			QXTIStatusA.ActualState +=1;
			QXMIStatusA.ActualState +=1;
		}
		else
		{
			if(Port == UART1)
			{
				QXSTStatusB.ActualState +=1;
				QXTIStatusB.ActualState +=1;
				QXMIStatusB.ActualState +=1;
			}
		}
	}
	
	void SetEulerAnglesSection (char* BufferPortPtr,volatile char Port)
	{
		_iq valor;
		valor = _IQmpy(SetRollAngle,_IQ(RAD_TO_DEGREES));
		_IQtoa(( (BufferPortPtr) + (SET_ROLL * DATA_LENGTH)+5 ),"%4.2f",valor);
		*( (BufferPortPtr) + ((SET_ROLL*DATA_LENGTH)+DATA_LENGTH)+4 ) = ',';

		valor = _IQmpy(SetPitchAngle,_IQ(RAD_TO_DEGREES));
		_IQtoa(( (BufferPortPtr) + (SET_PITCH * DATA_LENGTH)+5 ),"%4.2f",valor);
		*( (BufferPortPtr) + ((SET_PITCH*DATA_LENGTH)+DATA_LENGTH)+4) = ',';

		valor = _IQmpy(SetYawAngle,_IQ(RAD_TO_DEGREES));
		_IQtoa(( (BufferPortPtr) + (SET_YAW * DATA_LENGTH)+5 ),"%4.2f",valor);
		*( (BufferPortPtr) + ((SET_YAW*DATA_LENGTH)+DATA_LENGTH)+4) = ',';
		
		// Next State	//
		if(Port == UART0)
		{
			QXSTStatusA.ActualState +=1;
			QXTIStatusA.ActualState +=1;
			QXMIStatusA.ActualState +=1;
		}
		else
		{
			if(Port == UART1)
			{
				QXSTStatusB.ActualState +=1;
				QXTIStatusB.ActualState +=1;
				QXMIStatusB.ActualState +=1;
			}
		}
	}
	
	void HeightSection (char* BufferPortPtr,volatile char Port)
	{
		sprintf(( (BufferPortPtr) + (HEIGHT * DATA_LENGTH)+5 ),"%d",HeightCM_t);
		*( (BufferPortPtr) + ((HEIGHT*DATA_LENGTH)+DATA_LENGTH)+4 ) = ',';
		sprintf(( (BufferPortPtr) + (SET_HEIGHT * DATA_LENGTH)+5 ),"%d",0);
		*( (BufferPortPtr) + ((SET_HEIGHT*DATA_LENGTH)+DATA_LENGTH)+4 ) = ',';
		
		/*	Next State	*/
		if(Port == UART0)
		{
			QXSTStatusA.ActualState +=1;
			QXTIStatusA.ActualState +=1;
			QXMIStatusA.ActualState +=1;
		}
		else
		{
			if(Port == UART1)
			{
				QXSTStatusB.ActualState +=1;
				QXTIStatusB.ActualState +=1;
				QXMIStatusB.ActualState +=1;
			}
		}
	}
	
	void OnBoardTemperatureSection (char* BufferPortPtr,volatile char Port)
	{
		VfnDS16XXTemperatureToString(( (BufferPortPtr) + (BOARD_TEMPERATURE * DATA_LENGTH)+5 ),DS1624Temp);	
		*( (BufferPortPtr) + ((BOARD_TEMPERATURE*DATA_LENGTH)+DATA_LENGTH)+4 ) = ',';

		/* next State	*/
		if(Port == UART0)
		{
			QXSTStatusA.ActualState +=1;
			QXTIStatusA.ActualState +=1;
			QXMIStatusA.ActualState +=1;
		}
		else
		{
			if(Port == UART1)
			{
				QXSTStatusB.ActualState +=1;
				QXTIStatusB.ActualState +=1;
				QXMIStatusB.ActualState +=1;
			}
		}
	}
	
	void BatterySection (char* BufferPortPtr,volatile char Port)
	{
		_IQtoa(( (BufferPortPtr) + (BATTERY * DATA_LENGTH)+5 ),"%4.2f",BatteryStatus_t);
		*( (BufferPortPtr) + ((BATTERY*DATA_LENGTH)+DATA_LENGTH)+4 ) = ',';
		
		/* Next State	*/
		if(Port == UART0)
		{
			QXSTStatusA.ActualState +=1;
			QXTIStatusA.ActualState +=1;
			QXINStatusA.ActualState +=1;
			QXDBStatusA.ActualState +=1;
			QXMIStatusA.ActualState +=1;
		}
		else
		{
			if(Port == UART1)
			{
				QXSTStatusB.ActualState +=1;
				QXTIStatusB.ActualState +=1;
				QXINStatusB.ActualState +=1;
				QXDBStatusB.ActualState +=1;
				QXMIStatusB.ActualState +=1;
			}
		}
	}
	
	void MotorsThrustSection (char* BufferPortPtr,volatile char Port)
	{
		unsigned char thrust;
		volatile unsigned short Motors[4];
		thrust =  VfnGetQuadThrust();
		sprintf(( (BufferPortPtr) +(THRUST * DATA_LENGTH)+5 ),"%d",thrust);
		*( (BufferPortPtr) + ((THRUST*DATA_LENGTH)+DATA_LENGTH)+4 ) = ',';
		sprintf(( (BufferPortPtr) +(SET_THRUST * DATA_LENGTH)+5 ),"%d",0);
		*( (BufferPortPtr) + ((SET_THRUST*DATA_LENGTH)+DATA_LENGTH)+4 ) = ',';

		VfnGetMotorsThrust(&Motors[0]);
		sprintf(( (BufferPortPtr) +(MOTOR_X1 * DATA_LENGTH)+5 ),"%d",Motors[0]);
		*( (BufferPortPtr) + ((MOTOR_X1*DATA_LENGTH)+DATA_LENGTH)+4) = ',';
		sprintf(( (BufferPortPtr) +(MOTOR_X2 * DATA_LENGTH)+5 ),"%d",Motors[1]);
		*( (BufferPortPtr) + ((MOTOR_X2*DATA_LENGTH)+DATA_LENGTH)+4) = ',';
		sprintf(( (BufferPortPtr) + (MOTOR_Y1 * DATA_LENGTH)+5 ),"%d",Motors[2]);
		*( (BufferPortPtr) + ((MOTOR_Y1*DATA_LENGTH)+DATA_LENGTH)+4) = ',';
		sprintf(( (BufferPortPtr) + (MOTOR_Y2 * DATA_LENGTH)+5 ),"%d",Motors[3]);
		*( (BufferPortPtr) + ((MOTOR_Y2*DATA_LENGTH)+DATA_LENGTH)+4) = ',';
		
		/* Next State	*/
		if(Port == UART0)
		{
			QXSTStatusA.ActualState +=1;
			QXTIStatusA.ActualState +=1;
			QXMIStatusA.ActualState +=1;
		}
		else
		{
			if(Port == UART1)
			{
				QXSTStatusB.ActualState +=1;
				QXTIStatusB.ActualState +=1;
				QXMIStatusB.ActualState +=1;
			}
		}
	}
	
	void ControlTypeSection (char* BufferPortPtr,volatile char Port)
	{
		//  Falta el Control Type
		*( (BufferPortPtr) +(CONTROL_TYPE * DATA_LENGTH)+5) = '1';
		*( (BufferPortPtr) + ((CONTROL_TYPE*DATA_LENGTH)+DATA_LENGTH)+4) = ',';
		
		/*	Next State	*/
		if(Port == UART0)
		{
			QXSTStatusA.ActualState +=1;
			QXTIStatusA.ActualState +=1;
			QXMIStatusA.ActualState +=1;
		}
		else
		{
			if(Port == UART1)
			{
				QXSTStatusB.ActualState +=1;
				QXTIStatusB.ActualState +=1;
				QXMIStatusB.ActualState +=1;
			}
		}
	}
	
	void EnviromentSensorSection (char* BufferPortPtr,volatile char Port)
	{
		_iq value;
		sprintf(( (BufferPortPtr) + (CO_GAS * DATA_LENGTH)+5 ),"%d",COGas);
		*( (BufferPortPtr) + ((CO_GAS*DATA_LENGTH)+DATA_LENGTH)+4) = ',';
		sprintf(( (BufferPortPtr) + (O3_GAS * DATA_LENGTH)+5 ),"%d",O3Gas);
		*( (BufferPortPtr) + ((O3_GAS*DATA_LENGTH)+DATA_LENGTH)+4) = ',';

		value  = _IQ(EnvRHumidity.WholeNumber);
		value += _IQdiv(_IQ(EnvRHumidity.DecimalNumber),_IQ(100));
		_IQtoa(( (BufferPortPtr) +(HUMIDITY * DATA_LENGTH)+5 ),"%4.2f",value);
		*( (BufferPortPtr) + ((HUMIDITY*DATA_LENGTH)+DATA_LENGTH)+4) = ',';

		value  = _IQ(EnvTemperature.WholeNumber);
		value += _IQdiv(_IQ(EnvTemperature.DecimalNumber),_IQ(100));
		_IQtoa(( (BufferPortPtr) +(TEMPERATURE * DATA_LENGTH)+5 ),"%4.2f",value);
		*( (BufferPortPtr) + ((TEMPERATURE*DATA_LENGTH)+DATA_LENGTH)+4) = ',';
		
		/*	Next State	*/
		if(Port == UART0)
		{
			QXINStatusA.ActualState +=1;
			QXTIStatusA.ActualState +=1;
			QXMIStatusA.ActualState +=1;
		}
		else
		{
			if(Port == UART1)
			{
				QXINStatusB.ActualState +=1;
				QXTIStatusB.ActualState +=1;
				QXMIStatusB.ActualState +=1;
			}
		}
	}
	
	void PIDValuesSection (char* BufferPortPtr,volatile char Port)
	{
		_IQtoa(( (BufferPortPtr) + (PID_ROLL * DATA_LENGTH)+5 ),"%4.2f",PidR);
		*( (BufferPortPtr) +((PID_ROLL*DATA_LENGTH)+DATA_LENGTH)+4) = ',';
		_IQtoa(( (BufferPortPtr) + (PID_PITCH * DATA_LENGTH)+5 ),"%4.2f",PidP);
		*( (BufferPortPtr) +((PID_PITCH*DATA_LENGTH)+DATA_LENGTH)+4) = ',';
		_IQtoa(( (BufferPortPtr) + (PID_YAW * DATA_LENGTH)+5 ),"%4.2f",PidY);
		*( (BufferPortPtr) +((PID_YAW*DATA_LENGTH)+DATA_LENGTH)+4) = ',';	
		//Actializar variable del PID_HEIGHT
		_IQtoa(( (BufferPortPtr) + (PID_HEIGHT * DATA_LENGTH)+5 ),"%4.2f",_IQ(0.0));
		*( (BufferPortPtr) +((PID_HEIGHT*DATA_LENGTH)+DATA_LENGTH)+4) = ',';
		
		/*	Next State	*/
		if(Port == UART0)
		{
			QXDBStatusA.ActualState +=1;
			QXTIStatusA.ActualState +=1;
		}
		else
		{
			if(Port == UART1)
			{
				QXDBStatusB.ActualState +=1;
				QXTIStatusB.ActualState +=1;
			}
		}
	}
	
	void PIDErrorValuesSection (char* BufferPortPtr,volatile char Port)
	{
		_IQtoa(( (BufferPortPtr) + (ROLL_ERROR * DATA_LENGTH)+5 ),"%2.5f",ErrorRoll);
		*( (BufferPortPtr) +((ROLL_ERROR*DATA_LENGTH)+DATA_LENGTH)+4) = ',';
		_IQtoa(( (BufferPortPtr) + (PITCH_ERROR * DATA_LENGTH)+5 ),"%2.5f",ErrorPitch);
		*( (BufferPortPtr) +((PITCH_ERROR*DATA_LENGTH)+DATA_LENGTH)+4) = ',';
		_IQtoa(( (BufferPortPtr) + (YAW_ERROR * DATA_LENGTH)+5 ),"%2.5f",ErrorYaw);
		*( (BufferPortPtr) +((YAW_ERROR*DATA_LENGTH)+DATA_LENGTH)+4) = ',';
		
		/*	Next State	*/
		if(Port == UART0)
		{
			QXDBStatusA.ActualState +=1;
			QXTIStatusA.ActualState +=1;
		}
		else
		{
			if(Port == UART1)
			{
				QXDBStatusB.ActualState +=1;
				QXTIStatusB.ActualState +=1;
			}
		}	
	}

	// QXST Message Type
	void (* const FillQXSTTypeBuffer[]) (char* BufferPortPtr,volatile char Port)=
	{
		EulerAnlgesSection,
		SetEulerAnglesSection,
		HeightSection,
		OnBoardTemperatureSection,
		BatterySection,
		MotorsThrustSection,
		ControlTypeSection
	};
	// QXIN Message Type
	void (* const FillQXINTypeBuffer[]) (char* BufferPortPtr,volatile char Port) =
	{
		EnviromentSensorSection,
		BatterySection
	};
	// QXDB Message Type
	void (* const FillQXDBTypeBuffer[]) (char* BufferPortPtr,volatile char Port)=
	{
		PIDValuesSection,
		PIDErrorValuesSection,
		BatterySection
	};
	// QXTI Message Type
	void (* const FillQXTITypeBuffer[]) (char* BufferPortPtr,volatile char Port)=
	{
		EulerAnlgesSection,
		SetEulerAnglesSection,
		HeightSection,
		OnBoardTemperatureSection,
		BatterySection,
		MotorsThrustSection,
		ControlTypeSection,
		EnviromentSensorSection,
		PIDValuesSection,
		PIDErrorValuesSection
	};
	
	void (* const FillQXMITypeBuffer[] ) (char* BufferPortPtr,volatile char Port) =
	{
		EulerAnlgesSection,
		SetEulerAnglesSection,
		HeightSection,
		OnBoardTemperatureSection,
		BatterySection,
		MotorsThrustSection,
		ControlTypeSection,
		EnviromentSensorSection
	};
	
	void VfnMessageLimiters (char *DataGUIBufferPtr)
	{
		*(DataGUIBufferPtr)     = '<';
		*((DataGUIBufferPtr)+1) = 'Q';
		*((DataGUIBufferPtr)+2) = 'X';
		if( MessageTypeA == QXST )
		{
			*((DataGUIBufferPtr)+3)='S';
			*((DataGUIBufferPtr)+4)='T';
		}
		if( MessageTypeA == QXIN )
		{
			*((DataGUIBufferPtr)+3)='I';
			*((DataGUIBufferPtr)+4)='N';
		}
		if( MessageTypeA == QXDB )
		{
			*((DataGUIBufferPtr)+3)='D';
			*((DataGUIBufferPtr)+4)='B';
		}
		if( MessageTypeA == QXTI )
		{
			*((DataGUIBufferPtr)+3)='T';
			*((DataGUIBufferPtr)+4)='I';
		}
		if( MessageTypeA == QXMI )
		{
			*((DataGUIBufferPtr)+3)='M';
			*((DataGUIBufferPtr)+4)='I';
		}
		*((DataGUIBufferPtr)+5) = ',';
		*((DataGUIBufferPtr)+(MESSAGE_LENGTH-1)) = '>';
	}
#endif

/*****************************************************************************
 * Function:	VfnGUIBufferErase
 * Description:	Function that Erase DataGUIBuffer and, be ready to be fill it with another Message Type.
 * Parameters:	
 * Caveats:		
 * Return:		N/A
 *****************************************************************************/
	void VfnGUIBufferErase (char *DataGUIBufferPtr)
	{
		long Counter=0;
		while(Counter != MESSAGE_LENGTH)
		{
			Counter++;
			*DataGUIBufferPtr = 0;
			DataGUIBufferPtr++;	
		}
	}
	
/*****************************************************************************
 * Function:	VfnParsedData
 * Description:	Function that split the Messag and, gives number of data splited and position
 * Parameters:	*BufferPtr -> Pointer to the Data buffer to be parsed
 * 				Slpiter    -> character that will split data
 * 				*DataCounter  -> Counts the number of data sliped
 * 				*DataPosition -> Buffer cointaning the beginning of the each data
 * Caveats:		
 * Return:		N/A
 *****************************************************************************/
	void VfnParsedData(char *BufferPtr, unsigned char ParseCounter,char Spliter, unsigned char* DataCounter, char *DataPosition)
	{
		unsigned char Counter=0;
		*DataPosition = Counter;
		DataPosition++;
		while( Counter != ParseCounter )	
		{
			if(*BufferPtr == Spliter)
			{
				*DataPosition = Counter + 1;
				DataPosition++;
				BufferPtr++;
				*DataCounter +=1;	
			}
			else
			{	
				BufferPtr++;
			}
			Counter++;
		}
	}
